import numpy as np
from math import pi
from scipy.spatial.transform import Rotation as Rot

np.set_printoptions(suppress=True)

DYNAMIC = False
STATIC = True

# 齐次变换到笛卡尔坐标
def transfrom2RPY(Trans,mode = "xyz"):
  Trans = np.matrix(Trans)
  R = Trans[:3,:3]
  Eur = Rot.from_matrix(R)
  Eur = Eur.as_euler(mode,False)
  T = np.matrix(Trans[:3,3]).T
  T = np.array(T)
  return np.hstack((list(T[0]),Eur))

# 算法输出到齐次变换
def getRR(R,T):
  R = np.array(R)
  T = np.matrix(T)
  E = [[ 0, -1 , 0],
      [ 0 , 0 ,-1],
      [ 1 , 0,  0]]
  E = np.array(E)

  R_fixed = np.matmul(R,E.T) # 修正之后的R

  res = np.vstack((np.hstack((R_fixed,T.T)),np.array([0,0,0,1])))

  return res

# 获取齐次变换矩阵
# Euler : 角度
# T ： 平移
def getTransform(m:str,Euler,T,Mode:bool):
  if Mode == True:
      m = m.lower()
  else:
      m = m.upper()
  E = np.array(Euler)

  T = np.matrix(T)

  r = Rot.from_euler(m,E)

  return np.vstack((np.hstack((r.as_matrix(),T.T)),np.array([0,0,0,1])))

def finally_succ(curPos, RR, TT, Bais=[-33,-78,-180]):
  TT =np.array(TT)*1000 + np.array(Bais)
  Trans1 = getTransform("xyz",curPos[3:],np.array(curPos[:3]),STATIC) # on base the tool
  Trans2 = getRR(RR,TT) # on camera the target
  Trans3 = np.dot(Trans1,Trans2) # on base the target

  return transfrom2RPY(Trans3)

def GG_2_Liner_move(gg, curPos, toolBais=[-33,-78,-140]):
    RR = list(gg.rotation_matrices)[0]
    print(RR)
    TT = list(gg.translations)
    return finally_succ(curPos,RR,TT,toolBais)
